#include "Echo.h"
#include <EEPROM.h>
#include "HardwareSerial.h"
#include <MIDI.h>

Echo::Echo(MIDI *midi) {
  this->midi = midi;
  time = 0;
  feedback = 0;
  notec = 0;
  syncc = 0;
  holdc = 0;
  echoc = 0;
  midisync = false;
}

void Echo::readFromSerial() {
  if (Serial.available() > 0) {
    time = Serial.read();
    delayMicroseconds(4250);
    feedback = Serial.read();
    delayMicroseconds(4250);
    int x = Serial.read();
    if (x == 0) midisync = false;
    else midisync = true;
  }  
}

void Echo::writeToSerial() {
  Serial.write(time);
  Serial.write(feedback);
  if (midisync) {
    byte b = 1;
    Serial.write(b);
  }
  else {
    byte b = 0;
    Serial.write(b);
  }  
}

void Echo::readFromMemory(int index) {
  time = EEPROM.read(index);
  feedback = EEPROM.read(index + 1);
  int x = EEPROM.read(index + 2);
  if (x == 0) midisync = false;
  else midisync = true;
}

void Echo::writeToMemory(int index) {
  EEPROM.write(index, time);
  EEPROM.write(index + 1, feedback);
  if (midisync) EEPROM.write(index + 2, 1);
  else EEPROM.write(index + 2, 0);
}

void Echo::handleNoteOn() {
  midi->send(midi->getStatus(), midi->getData1(), midi->getData2());
  if (notec < 32) {
    notes[notec][0] = midi->getStatus();
    notes[notec][1] = midi->getData1();
    notes[notec][2] = midi->getData2();
    notelengths[notec] = millis();
    notes[notec][3] = 96 / 100 * feedback;
    if (midisync) {
      notes[notec][4] = syncc + (24 / (time / 4)); // Next sync count the note should play syncc + (24 / (time / 4))      
    }
    else {
      notetime[notec] = millis() + time;  
    }
    hold[holdc] = notec; 
    holdc++;
    notec++;    
  }
}

void Echo::handleNoteOff() {
  midi->send(midi->getStatus(), midi->getData1(), midi->getData2());
  if (holdc > 0) { // should always be true!
    for (int i = 0; i < holdc; i++) {
      if (midi->getData1() == notes[hold[i]][1]) {
        notelengths[i] = millis() - notelengths[i]; // set note length
        // remove from hold[]
        if (i < holdc - 1) { // not the last or first one
          while (i < holdc) { // copy the rest one row down
            hold[i] = hold[i + 1];
            i++;
          }
        }
        holdc--;
        break;
      }
    }
  }  
}

void Echo::sync() {
  syncc++;
  for (int i = 0; i < notec; i++) { // check for note on
    if (notes[i][5] == syncc) {
      midi->send(notes[i][0], notes[i][1], notes[i][2]); // TODO: Lower velocity
      notes[i][3]--; // if 0 note will be removed after note off
      notes[i][4] = syncc + 24 / (time / 4);
      echo[echoc] = i;
      echotime[echoc] = millis();
      echoc++;
    }
  }
  for (int i = 0; i < echoc; i++) { // check for note off
    if (notelengths[echo[i]] >= millis() - echotime[i]) {
      midi->send(notes[echo[i]][0] - 0x10, notes[echo[i]][1], notes[echo[i]][2]); // TODO: handle velocity
      if (notes[echo[i]][3] == 0) { // remove note from notes
        if (echo[i] < notec - 1) { // not last or first
          for (int j = echo[i]; j < notec; j++) {
            notes[j][0] = notes[j + 1][0];
            notes[j][1] = notes[j + 1][1];
            notes[j][2] = notes[j + 1][2];
            notes[j][3] = notes[j + 1][3];
            notes[j][4] = notes[j + 1][4];
            notelengths[j] = notelengths[j + 1];
          }
        }
        notec--;
      }
      // remove from echo[]
      if (i < echoc - 1) {
        for (int j = i; j < echoc; j++) {
          echo[j] = echo[j + 1];
          echotime[j] = echotime[j + 1];
        }
      }
      echoc--;      
    }
  }  
}
